#include "internal.h"

#define ROUNDUP(a, b) (((a) + ((b) - 1)) & ~((b) - 1))

#define ORDER_TO_PAGES(order) (1 << (order))
#define INDEX_TO_ORDER(zone, idx) min(LowBitGet(idx), (MAX_PAGE_ORDER - 1))

#ifndef CTZ
#define CTZ(value) (__builtin_ctz(value))
#endif

#ifndef CLZ
#define CLZ(value) (__builtin_clz(value))
#endif

static inline uint16_t HighBitGet(uint32_t bitmap)
{
    if (bitmap == 0)
    {
        return 32;
    }

    return (0x1f - CLZ(bitmap));
}

static inline uint16_t LowBitGet(uint32_t bitmap)
{
    if (bitmap == 0)
    {
        return 32;
    }

    return CTZ(bitmap);
}

static inline void PageAddUnsafe(MemZone_t *zone, Page_t *page, uint8_t order)
{
    __add_to_free_list(page, zone, order, 0, true);
}

static inline void PageDelUnsafe(MemZone_t *zone, Page_t *page)
{
    __del_page_from_free_list(page, zone, page->order, 0);

    page->order = MAX_PAGE_ORDER;
}

static void PagesSpiltUnsafe(MemZone_t *zone, Page_t *page, uint8_t oldOrder, uint8_t newOrder)
{
    Page_t *buddyPage = NULL;
    uint32_t order;

    for (order = newOrder; order > oldOrder;)
    {
        order--;
        buddyPage = &page[ORDER_TO_PAGES(order)];

        PageAddUnsafe(zone, buddyPage, order);
    }
}

static inline void PagesRecycleExtra(MemZone_t *zone, Page_t *page, size_t startPage, size_t endPage)
{
    if (startPage >= endPage)
    {
        return;
    }

    OsPagesFreeContiguous(zone, page, endPage - startPage);
}

static Page_t *PagesLargeAlloc(MemZone_t *zone, size_t nPages)
{
    uintptr_t start;
    uintptr_t end;    
    FreeArea_t *area;
    Page_t *page;
    Page_t *tmp;
    Page_t *ret = NULL;

    area = __zone_freearea(zone, MAX_PAGE_ORDER - 1);
    __for_each_page_in_area(area, page)
    {
        start = __page_index(zone, page);
        end = start + nPages;
        if (end > __zone_spanned_pages(zone))
        {
            continue;
        }

        for (;;)
        {
            start += (MAX_PAGE_ORDER - 1);
            if ((start >= end) || (start > __zone_spanned_pages(zone)))
            {
                break;
            }

            tmp = __zone_page_at(zone, start);
            if (tmp->order != (MAX_PAGE_ORDER - 1))
            {
                break;
            }
        }

        if (start >= end)
        {
            ret = page;
            break;
        }
    }

    return ret;
}

static inline uint32_t PagesToOrder(uint32_t nPages)
{
    uint32_t order;

    for (order = 0; ORDER_TO_PAGES(order) < nPages; order++)
        ;

    return order;
}

static Page_t *__PagesAlloc(MemZone_t *zone, size_t nPages)
{
    FreeArea_t *area = NULL;
    Page_t *page = NULL;
    Page_t *tmp = NULL;
    uint32_t order;
    uint32_t newOrder;

    order = PagesToOrder(nPages);
    if (order < MAX_PAGE_ORDER)
    {
        for (newOrder = order; newOrder < MAX_PAGE_ORDER; newOrder++)
        {
            area = __zone_freearea(zone, newOrder);
            if (__area_is_empty(area))
            {
                continue;
            }
            page = __area_first_page(area);
            goto DONE;
        }
    }
    else
    {
        newOrder = MAX_PAGE_ORDER - 1;
        page = PagesLargeAlloc(zone, nPages);
        if (page != NULL)
        {
            goto DONE;
        }
    }
    return NULL;
DONE:

    for (tmp = page; tmp < &page[nPages]; tmp = &tmp[1 << newOrder])
    {
        PageDelUnsafe(zone, tmp);
    }
    PagesSpiltUnsafe(zone, page, order, newOrder);
    PagesRecycleExtra(zone, &page[nPages], nPages, ROUNDUP(nPages, (1 << min(order, newOrder))));

    return page;
}

static Page_t *PagesAlloc(MemZone_t *zone, size_t nPages)
{
    Page_t *page;

    page = __PagesAlloc(zone, nPages);

    return page;
}

static void PagesFree(MemZone_t *zone, Page_t *page, uint8_t order)
{
    uintptr_t index;
    Page_t *buddyPage = NULL;

    if ((page == NULL) || (order >= MAX_PAGE_ORDER))
    {
        return;
    }

    if (order < MAX_PAGE_ORDER - 1)
    {
        index = __page_index(zone, page);
        do
        {
            index ^= (1 << order);
            buddyPage = __zone_page_at(zone, index);
            if ((buddyPage == NULL) || (buddyPage->order != order))
            {
                break;
            }
            PageDelUnsafe(zone, buddyPage);
            order++;
            index &= ~((1 << order) - 1);
            page = __zone_page_at(zone, index);
        } while (order < (MAX_PAGE_ORDER - 1));
    }

    PageAddUnsafe(zone, page, order);
}

void OsPagesFreeContiguous(MemZone_t *zone, Page_t *page, size_t nPages)
{
    uintptr_t index;
    size_t n;
    uint32_t order;

    while (1)
    {
        index = __page_index(zone, page);
        order = INDEX_TO_ORDER(zone, index);
        n = ORDER_TO_PAGES(order);
        if (n > nPages)
        {
            break;
        }

        PagesFree(zone, page, order);
        nPages -= n;
        page += n;
    }

    while (nPages > 0)
    {
        order = HighBitGet(nPages);
        PagesFree(zone, page, order);

        n = ORDER_TO_PAGES(order);
        nPages -= n;
        page += n;
    }
}

Page_t *OsPagesAllocContiguous(MemZone_t *zone, size_t nPages)
{
    Page_t *page = NULL;

    if (nPages != 0)
    {
        page = PagesAlloc(zone, nPages);
    }

    return page;
}
